﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ProyectoSI;
using System.Collections;
using VisionEstereo.SegmentacionColores;

namespace VisionEstereo.AlgoritmoCorrelacion
{
    public unsafe class AlgCorrelacion
    {
        int max(int a, int b) { if (a > b) return a; return b; }
        int min(int a, int b) { if (a < b) return a; return b; }

        int dimx, dimy;
        int i, j, k, l, cl, cu, rl, ru;
        double[] pen;
     
        
        Bitmap imagenSalida;
        BitmapFast mapaDisparidad, imagenSegmentada;



        BitmapFast.PixelData* c;
        BitmapFast.PixelData* d;
        PixelDato color, disp;

        public Bitmap EjecutarAlgCorrelacion(Bitmap mapaDisp, Bitmap imagenSegm, int[,] mapaDisparidades, int[,] mapaClases)
        {

            mapaDisparidad = new BitmapFast(mapaDisp);
            imagenSegmentada = new BitmapFast(imagenSegm);
            imagenSegmentada.LockBitmap();
            mapaDisparidad.LockBitmap();                    
       
            ElegirFiltroCorrelacion ventanaFiltro2 = new ElegirFiltroCorrelacion();

            if (ventanaFiltro2.ShowDialog() == true)
            {

                int tam = Int32.Parse(ventanaFiltro2.tamVentanaBox.Text);   
                
                
                dimx = mapaDisparidad.Width;
                dimy = mapaDisparidad.Height;
                Bitmap bitmapAux = new Bitmap(dimx,dimy);
                PixelDato[,] matrizSalida = new PixelDato[dimx, dimy];
                int[] win = new int[2];

                pen = new double[256];

            
                win[0] = tam;
                win[1] = tam;

                for (i = 0; i < dimx; i++)        // para cada pixel
                {
                    for (j = 0; j < dimy; j++)
                    {
            
                            //recalcula los límites para que la ventana no se salga de rango
                            rl = max(0, i - win[0]);
                            ru = min((int)dimx, i + win[0]);
                            cl = max(0, j - win[1]);
                            cu = min((int)dimy, j + win[1]);                            
                                    
                            /* para cada ventana aplicamos el filtro elegido*/
                            if(ventanaFiltro2.Moda.IsChecked==true){
     
                           
                                    FiltroModa(i,j,tam,rl,ru,cl,cu,mapaDisparidades,mapaClases, matrizSalida);
                               
                            }
                            else if (ventanaFiltro2.Media.IsChecked==true){
                                    FiltroMedia(bitmapAux,i,j,tam, rl, ru, cl, cu, mapaDisparidades, mapaClases, matrizSalida);
                             
                            }
                            else{
                                    FiltroMediana(bitmapAux,i,j,tam,rl,ru,cl,cu,mapaDisparidades,mapaClases, matrizSalida);
                            
                            }                                              
                                                                        
                    }
                }

                mapaDisparidad.UnlockBitmap();
                imagenSegmentada.UnlockBitmap();
                if ((ventanaFiltro2.Media.IsChecked==true)||(ventanaFiltro2.Mediana.IsChecked==true))
                {
                    return ProyectoSI.ColorMapDisparidades.ObtenerColorMap(bitmapAux, 15);
                }
                else
                //dibujar nueva matriz
                    imagenSalida = new Bitmap(mapaDisp, mapaDisp.Size);
                for (i = 0; i < dimx; i++)
                {
                    for (j = 0; j < dimy; j++)
                    {
                        Color color_aux = Color.FromArgb(matrizSalida[i, j].Red, matrizSalida[i, j].Green, matrizSalida[i, j].Blue);
                        imagenSalida.SetPixel(i, j, color_aux);
                    }
                }
                
                return imagenSalida;
            }
            else return null;            

        }

        private unsafe void FiltroMediana(Bitmap bitmapAux, int i, int j,int tam, int rl, int ru, int cl, int cu, int[,] mapaDisparidades, int[,] mapaClases, PixelDato[,] matrizSalida)
        {
            /*recorrer ventana
             * guardar disparidades de pixeles del mismo color q el central
             * N++
             * ordenar vector
             * mediana = vector[N/2]
             */
            int colorCentro = mapaClases[i, j];
            ArrayList v = new ArrayList();
            int n = 0;
            for (k = rl; k < ru; k++)             // for every pixel in local window
            {
                for (l = cl; l < cu; l++)
                {
                    if (mapaClases[k, l] == colorCentro)
                    {
                        v.Add(mapaDisparidades[k,l]);
                        n = n + 1;
                    }
                }
            }
            v.Sort();
            int mitad=n/2;
            int mediana=(int)v[mitad];
            mapaDisparidades[i,j]=mediana;
            bitmapAux.SetPixel(i, j, Color.FromArgb(mediana, mediana, mediana));

        }

        private unsafe void FiltroMedia(Bitmap bitmapAux,int i,int j,int tam, int rl, int ru, int cl, int cu, int[,] mapaDisparidades, int[,] mapaClases, PixelDato[,] matrizSalida)
        {
            /*recorrer la venta
             * si es del mismo color q el pixel central
             *  sumar disparidad a la media acumulada
             *  cont++
             * media = suma / cont;
             */
            
            int colorCentro=mapaClases[i,j];
            int suma=0;
            int n=0;

            Hashtable temp = new Hashtable();
            for (k = rl; k < ru; k++)             // for every pixel in local window
            {
                for (l = cl; l < cu; l++)
                {
                    if (mapaClases[k, l] == colorCentro)
                    {
                        suma = suma + mapaDisparidades[k, l];
                        n = n + 1;

                    }
                }
            }
            int nuevaDispCentro = suma / n;
            mapaDisparidades[i, j] = nuevaDispCentro;
            bitmapAux.SetPixel(i, j, Color.FromArgb(nuevaDispCentro, nuevaDispCentro, nuevaDispCentro));
            
        }
               
        private void FiltroModa(int i, int j,int tam, int rl, int ru, int cl, int cu, int[,] mapaDisparidades, int[,] mapaClases, PixelDato[,] matrizSalida)
        {
            Hashtable clave_color = new Hashtable();
            Hashtable clave_disparidad = new Hashtable();

            
            int n = 0;
            PixelDato[] claveMejor = new PixelDato[2];
            int modaMejor = 0;
            int mejor_disp = mapaDisparidades[i, j];
           // int shaft = tam / 2;
            int vecesCentro = 0;
            Hashtable clave_disparidadCentro = null;

            for (k = rl; k < ru; k++)             // for every pixel in local window
            {
                for (l = cl; l < cu; l++)
                {
                    n = 1;
                    c = imagenSegmentada.PixelAt(k, l);
                    d = mapaDisparidad.PixelAt(k, l);
                    color = new PixelDato(c->blue, c->green, c->red);
                    disp = new PixelDato(d->blue, d->green, d->red);

                    if (clave_color.Contains(mapaClases[k, l]))
                    {
                        clave_disparidad = (Hashtable)clave_color[mapaClases[k, l]];
                        if (clave_disparidad.Contains(mapaDisparidades[k, l]))
                        {
                            n = (int)clave_disparidad[mapaDisparidades[k, l]];
                            n++;
                            clave_disparidad.Remove(mapaDisparidades[k, l]);
                            clave_disparidad.Add(mapaDisparidades[k, l], n);
                            clave_color.Remove(mapaClases[k, l]);
                            clave_color.Add(mapaClases[k, l], clave_disparidad);

                        }
                        else
                        {
                            clave_disparidad.Add(mapaDisparidades[k, l], n);
                            clave_color.Remove(mapaClases[k, l]);
                            clave_color.Add(mapaClases[k, l], clave_disparidad);
                        }
                    } //fsi
                    else
                    {

                        Hashtable dis = new Hashtable();
                        dis.Add(mapaDisparidades[k, l], n);
                        clave_color.Add(mapaClases[k, l], dis);
                    } //felse
                    if (n > modaMejor)
                    {
                        modaMejor = n;
                        claveMejor[0] = color;
                        claveMejor[1] = disp;
                        mejor_disp = mapaDisparidades[k, l];
                    }
                }// ffor
                
            }//ffor

            clave_disparidadCentro = (Hashtable)clave_color[mapaClases[i, j]];
            vecesCentro = (int)clave_disparidadCentro[mapaDisparidades[i, j]];

            if (vecesCentro < modaMejor)
            {
                matrizSalida[i, j] = claveMejor[1];              // set output pixel to mode from window
                mapaDisparidades[i, j] = mejor_disp;
            }
            else
            {
                c = mapaDisparidad.PixelAt(i, j);
                color = new PixelDato(c->blue, c->green, c->red);
                matrizSalida[i, j] = color;
            }

            
        }//fModa

    } //fclass 
}//fnamespace
